﻿using System;
using System.Collections.Generic;
using System.Drawing;
using System.Text;

namespace  Sharp3DMath.Core
{
    public enum FitMode
    {
        FPF,
        TAUBIN,
        BOOKSTEIN
    }
    public static partial class MathEx
    {
        private static String warn_taub_str = "Sorry! Taubin method not yet implemented.";

        public static Vector2F[] ConvertToVector2FArray(Point[] points)
        {
            Vector2F[] repoints = new Vector2F[points.Length];
            for (int i = 0; i < points.Length; i++)
            {
                repoints[i] = new Vector2F(points[i]);
            }
            return repoints;
        }
        public static Vector2F[] ConvertToVector2FArray(PointF[] points)
        {
            Vector2F[] repoints = new Vector2F[points.Length];
            for (int i = 0; i < points.Length; i++)
            {
                repoints[i] = new Vector2F(points[i]);
            }
            return repoints;
        }
        public static Point[] ConvertToPointArray(Vector2F[] points)
        {
            Point[] repoints = new Point[points.Length];
            for (int i = 0; i < points.Length; i++)
            {
                repoints[i] = new Point(MathEx.Round(points[i].X),MathEx.Round(points[i].Y));
            }
            return repoints;
        }
        #region
        public static Vector2F[] EllipsePaint(Graphics graphics,Vector2F[] points,FitMode mode,int fitTolerant)
        {
            return ConvertToVector2FArray(EllipsePaint(graphics, MathEx.ConvertToPointArray(points), mode,fitTolerant));
        }
        public static Point[] EllipsePaint(Graphics graphics, Point[] points, FitMode mode, int fitTolerant)
        {
            int np = points.Length;
            if (np < 6)
                throw new ArgumentException("must more than 6 points");

            double[,] D = new double[np+1,7];
            double[,] S = new double[7, 7];
            double[,] Const = new double[7, 7];
            double[,] temp = new double[7, 7];
            double[,] L = new double[7, 7];
            double[,] C = new double[7, 7];

            double[,] invL = new double[7, 7];
            double[] d = new double[7];
            double[,] V = new double[7, 7];
            double[,] sol = new double[7, 7];
            double tx, ty;
            int nrot = 0;
            int npts = fitTolerant;

            double[,] XY = new double[3, npts + 1];
            double[] pvec = new double[7];

            switch (mode)
            {
                case FitMode.FPF:
                    Console.WriteLine("FPF Mode");
                    Const[1,3] = -2;
                    Const[2,2] = 1;
                    Const[3,1] = -2;
                    break;
                case FitMode.TAUBIN:
                    Console.WriteLine(warn_taub_str);
                    break;
                case FitMode.BOOKSTEIN:
                    Console.WriteLine("BOOKSTEIN Mode");
                    Const[1, 1] = 2;
                    Const[2, 2] = 1;
                    Const[3, 3] = 2;
                    break;
                default:
                    break;
            }

            
            // Now first fill design matrix
            for (int i = 1; i <= np; i++)
            {
                tx = points[i - 1].X;
                ty = points[i - 1].Y;
                D[i,1] = tx * tx;
                D[i,2] = tx * ty;
                D[i,3] = ty * ty;
                D[i,4] = tx;
                D[i,5] = ty;
                D[i,6] = 1.0;
            }


           // pm(Const, "Constraint");
            // Now compute scatter matrix  S
            A_TperB(D, D, S, np, 6, np, 6);
            //pm(S,"Scatter");

            choldc(S, 6, L);
            //pm(L,"Cholesky");

            inverse(L, invL, 6);
            //pm(invL,"inverse");

            AperB_T(Const, invL, temp, 6, 6, 6, 6);
            AperB(invL, temp, C, 6, 6, 6, 6);
           // pm(C,"The C matrix");

            jacobi(C, 6, d, V, nrot);
            //pm(V,"The Eigenvectors");  /* OK */
            //pv(d,"The eigevalues");

            A_TperB(invL, V, sol, 6, 6, 6, 6);
            //pm(sol, "The GEV solution unnormalized");  /* SOl */
            
            // Now normalize them 
            for (int j = 1; j <= 6; j++)  /* Scan columns */
            {
                double mod = 0.0;
                for (int i = 1; i <= 6; i++)
                    mod += sol[i,j] * sol[i,j];
                for (int i = 1; i <= 6; i++)
                    sol[i,j] /= Math.Sqrt(mod);
            }

            //pm(sol, "The GEV solution");  /* SOl */

            double zero = double.Epsilon;//10e-20;
            double minev = double.MaxValue; //10e+20;
            int solind = 0;

            switch (mode)
            {
                case FitMode.BOOKSTEIN:  // smallest eigenvalue	 	   
                    for (int i = 1; i <= 6; i++)
                        if (d[i] < minev && Math.Abs(d[i]) > zero)
                            solind = i;
                    break;
                case FitMode.FPF:
                    for (int i = 1; i <= 6; i++)
                        if (d[i] < 0 && Math.Abs(d[i]) > zero)
                            solind = i;
                    break;
            }

            // Now fetch the right solution
            for (int j = 1; j <= 6; j++)
                pvec[j] = sol[j,solind];
            //pv(pvec, "the solution");

            // ...and plot it
            draw_conic(pvec, npts, XY);

            List<Point> temp_points = new List<Point>();
            for (int i = 1; i < npts; i++)
            {
                if (XY[1, i] == -1 || XY[1, i + 1] == -1)
                    continue;
                else
                    if (i < npts)
                    {
                        temp_points.Add(new Point((int)XY[1, i], (int)XY[2, i]));

                    }
                //graphics.DrawLine(Pens.Black,(int)XY[1,i], (int)XY[2,i], (int)XY[1,i + 1], (int)XY[2,i + 1]);
                //bug,never happend
                //graphics.DrawLine(Pens.Black,(int)XY[1, i], (int)XY[2, i], (int)XY[1, 1], (int)XY[2, 1]);
            }
            return temp_points.ToArray();
        }


        private static void draw_conic(double[] pvec, int nptsk, double[,] points)
        {
            int npts = nptsk / 2;
            double[,] u = new double[3, npts + 1];
            double[,] Aiu = new double[3, npts + 1];
            double[,] L = new double[3, npts + 1];
            double[,] B = new double[3, npts + 1];
            double[,] Xpos = new double[3, npts + 1];
            double[,] Xneg = new double[3, npts + 1];
            double[,] ss1 = new double[3, npts + 1];
            double[,] ss2 = new double[3, npts + 1];
            double[] lambda = new double[npts + 1];
            double[,] uAiu = new double[3, npts + 1];
            double[,] A = new double[3, 3];
            double[,] Ai = new double[3, 3];
            double[,] Aib = new double[3, 2];
            double[,] b = new double[3, 2];
            double[,] r1 = new double[2, 2];
            double Ao, Ax, Ay, Axx, Ayy, Axy;

            double pi = 3.14781;
            double theta;
            int i;
            int j;
            double kk;

            Ao = pvec[6];
            Ax = pvec[4];
            Ay = pvec[5];
            Axx = pvec[1];
            Ayy = pvec[3];
            Axy = pvec[2];

            A[1, 1] = Axx; A[1, 2] = Axy / 2;
            A[2, 1] = Axy / 2; A[2, 2] = Ayy;
            b[1, 1] = Ax; b[2, 1] = Ay;

            // Generate normals linspace
            for (i = 1, theta = 0.0; i <= npts; i++, theta += (pi / npts))
            {
                u[1, i] = Math.Cos(theta);
                u[2, i] = Math.Sin(theta);
            }

            inverse(A, Ai, 2);

            AperB(Ai, b, Aib, 2, 2, 2, 1);
            A_TperB(b, Aib, r1, 2, 1, 2, 1);
            r1[1, 1] = r1[1, 1] - 4 * Ao;

            AperB(Ai, u, Aiu, 2, 2, 2, npts);
            for (i = 1; i <= 2; i++)
                for (j = 1; j <= npts; j++)
                    uAiu[i, j] = u[i, j] * Aiu[i, j];

            for (j = 1; j <= npts; j++)
            {
                if ((kk = (r1[1, 1] / (uAiu[1, j] + uAiu[2, j]))) >= 0.0)
                    lambda[j] = Math.Sqrt(kk);
                else
                    lambda[j] = -1.0;
            }

            // Builds up B and L
            for (j = 1; j <= npts; j++)
                L[1, j] = L[2, j] = lambda[j];
            for (j = 1; j <= npts; j++)
            {
                B[1, j] = b[1, 1];
                B[2, j] = b[2, 1];
            }

            for (j = 1; j <= npts; j++)
            {
                ss1[1, j] = 0.5 * (L[1, j] * u[1, j] - B[1, j]);
                ss1[2, j] = 0.5 * (L[2, j] * u[2, j] - B[2, j]);
                ss2[1, j] = 0.5 * (-L[1, j] * u[1, j] - B[1, j]);
                ss2[2, j] = 0.5 * (-L[2, j] * u[2, j] - B[2, j]);
            }

            AperB(Ai, ss1, Xpos, 2, 2, 2, npts);
            AperB(Ai, ss2, Xneg, 2, 2, 2, npts);

            for (j = 1; j <= npts; j++)
            {
                if (lambda[j] == -1.0)
                {
                    points[1, j] = -1.0;
                    points[2, j] = -1.0;
                    points[1, j + npts] = -1.0;
                    points[2, j + npts] = -1.0;
                }
                else
                {
                    points[1, j] = Xpos[1, j];
                    points[2, j] = Xpos[2, j];
                    points[1, j + npts] = Xneg[1, j];
                    points[2, j + npts] = Xneg[2, j];
                }
            }
        }
        #endregion

        #region Private Function
        private static void pm(double[,] S, String str)
        {
            System.Diagnostics.Debug.WriteLine("------------" + str + "--------------");
            System.Diagnostics.Debug.WriteLine(" " + S[1, 1] + " " + S[1, 2] + " " + S[1, 3]
                   + " " + S[1, 4] + " " + S[1, 5] + " " + S[1, 6]);

            System.Diagnostics.Debug.WriteLine(" " + S[2, 1] + " " + S[2, 2] + " " + S[2, 3]
                   + " " + S[2, 4] + " " + S[2, 5] + " " + S[2, 6]);

            System.Diagnostics.Debug.WriteLine(" " + S[3, 1] + " " + S[3, 2] + " " + S[3, 3]
                   + " " + S[3, 4] + " " + S[3, 5] + " " + S[3, 6]);

            System.Diagnostics.Debug.WriteLine(" " + S[4, 1] + " " + S[4, 2] + " " + S[4, 3]
                   + " " + S[4, 4] + " " + S[4, 5] + " " + S[4, 6]);

            System.Diagnostics.Debug.WriteLine(" " + S[5, 1] + " " + S[5, 2] + " " + S[5, 3]
                   + " " + S[5, 4] + " " + S[5, 5] + " " + S[5, 6]);

            System.Diagnostics.Debug.WriteLine(" " + S[6, 1] + " " + S[6, 2] + " " + S[6, 3]
                   + " " + S[6, 4] + " " + S[6, 5] + " " + S[6, 6]);

            System.Diagnostics.Debug.WriteLine("------------------------------------------");

        }

        private static void pv(double[] v, String str)
        {
            
            System.Diagnostics.Debug.WriteLine("------------" + str + "--------------");
            System.Diagnostics.Debug.WriteLine(" " + v[1] + " " + v[2] + " " + v[3]
                   + " " + v[4] + " " + v[5] + " " + v[6]);
            System.Diagnostics.Debug.WriteLine("------------------------------------------");
        }

        private static void A_TperB(double[,] _A, double[,] _B, double[,] _res,
                                    int _righA, int _colA, int _righB, int _colB)
        {
            int p, q, l;
            for (p = 1; p <= _colA; p++)
                for (q = 1; q <= _colB; q++)
                {
                    _res[p,q] = 0.0;
                    for (l = 1; l <= _righA; l++)
                        _res[p,q] = _res[p,q] + _A[l,p] * _B[l,q];
                }      

        }

        /// <summary>
        ///  Perform the Cholesky decomposition   
        ///  Return the lower triangular L  such that L*L'=A  
        /// </summary>
        /// <param name="a"></param>
        /// <param name="n"></param>
        /// <param name="l"></param>
        private static void choldc(double[,] a, int n, double[,] l)
        {
            int i, j, k;
            double sum;
            double[] p = new double[n + 1];

            for (i = 1; i <= n; i++)
            {
                for (j = i; j <= n; j++)
                {
                    for (sum = a[i, j], k = i - 1; k >= 1; k--) sum -= a[i, k] * a[j, k];
                    if (i == j)
                    {
                        if (sum <= 0.0)
                        // printf("\nA is not poitive definite!");
                        { }
                        else
                            p[i] = Math.Sqrt(sum);
                    }
                    else
                    {
                        a[j, i] = sum / p[i];
                    }
                }
            }
            for (i = 1; i <= n; i++)
                for (j = i; j <= n; j++)
                    if (i == j)
                        l[i, i] = p[i];
                    else
                    {
                        l[j, i] = a[j, i];
                        l[i, j] = 0.0;
                    }
        }

        /// <summary>
        /// 
        /// 
        /// 
        /// 
        ///  </summary>
        /// <param name="TB"></param>
        /// <param name="InvB"></param>
        /// <param name="N"></param>
        /// <returns></returns>
        private static int inverse(double[,] TB,double[,] InvB,int N)
        {
            int k, i, j, p, q;
            double mult;
            double D, temp;
            double maxpivot;
            int npivot;

            double[,] B = new double[N + 1, N + 2];
            double[,] A = new double[N + 1, 2 * N + 2];
            double[,] C = new double[N + 1, N + 1];

            double eps = double.Epsilon;//10e-20; 

            for (k = 1; k <= N; k++)
                for (j = 1; j <= N; j++)
                    B[k,j] = TB[k,j];

            for (k = 1; k <= N; k++)
            {
                for (j = 1; j <= N + 1; j++)
                    A[k,j] = B[k,j];
                for (j = N + 2; j <= 2 * N + 1; j++)
                    A[k,j] = (float)0;
                A[k,k - 1 + N + 2] = (float)1;
            }

            for (k = 1; k <= N; k++)
            {
                maxpivot = Math.Abs((double)A[k,k]);
                npivot = k;
                for (i = k; i <= N; i++)
                    if (maxpivot < Math.Abs((double)A[i,k]))
                    {
                        maxpivot = Math.Abs((double)A[i,k]);
                        npivot = i;
                    }
                if (maxpivot >= eps)
                {
                    if (npivot != k)
                        for (j = k; j <= 2 * N + 1; j++)
                        {
                            temp = A[npivot,j];
                            A[npivot,j] = A[k,j];
                            A[k,j] = temp;
                        };
                    D = A[k,k];
                    for (j = 2 * N + 1; j >= k; j--)
                        A[k,j] = A[k,j] / D;
                    for (i = 1; i <= N; i++)
                    {
                        if (i != k)
                        {
                            mult = A[i,k];
                            for (j = 2 * N + 1; j >= k; j--)
                                A[i,j] = A[i,j] - mult * A[k,j];
                        }
                    }
                }
                else
                {  // printf("\n The matrix may be singular !!") ;
                    return (-1);
                };
            }
            /**   Copia il risultato nella matrice InvB  ***/
            for (k = 1, p = 1; k <= N; k++, p++)
                for (j = N + 2, q = 1; j <= 2 * N + 1; j++, q++)
                    InvB[p,q] = A[k,j];
            return 0;
                /* End of INVERSE */
        }

        private static void AperB_T(double[,] _A, double[,] _B, double[,] _res,
             int _righA, int _colA, int _righB, int _colB)
        {
            int p, q, l;
            for (p = 1; p <= _colA; p++)
                for (q = 1; q <= _colB; q++)
                {
                    _res[p, q] = 0.0;
                    for (l = 1; l <= _righA; l++)
                        _res[p, q] = _res[p, q] + _A[p, l] * _B[q, l];
                }
        }

        private static void AperB(double[,] _A, double[,] _B, double[,] _res,
               int _righA, int _colA, int _righB, int _colB)
        {
            int p, q, l;
            for (p = 1; p <= _righA; p++)
                for (q = 1; q <= _colB; q++)
                {
                    _res[p, q] = 0.0;
                    for (l = 1; l <= _colA; l++)
                        _res[p, q] = _res[p, q] + _A[p, l] * _B[l, q];
                }
        }

        private static void jacobi(double[,] a, int n, double[] d, double[,] v, int nrot)
        {
            int j, iq, ip, i;
            double tresh, theta, tau, t, sm, s, h, g, c;

            double[] b = new double[n + 1];
            double[] z = new double[n + 1];

            for (ip = 1; ip <= n; ip++)
            {
                for (iq = 1; iq <= n; iq++) v[ip, iq] = 0.0;
                v[ip, ip] = 1.0;
            }
            for (ip = 1; ip <= n; ip++)
            {
                b[ip] = d[ip] = a[ip, ip];
                z[ip] = 0.0;
            }
            nrot = 0;
            for (i = 1; i <= 50; i++)
            {
                sm = 0.0;
                for (ip = 1; ip <= n - 1; ip++)
                {
                    for (iq = ip + 1; iq <= n; iq++)
                        sm += Math.Abs(a[ip, iq]);
                }
                if (sm == 0.0)
                {
                    /*    free_vector(z,1,n);
                      free_vector(b,1,n);  */
                    return;
                }
                if (i < 4)
                    tresh = 0.2 * sm / (n * n);
                else
                    tresh = 0.0;
                for (ip = 1; ip <= n - 1; ip++)
                {
                    for (iq = ip + 1; iq <= n; iq++)
                    {
                        g = 100.0 * Math.Abs(a[ip, iq]);
                        if (i > 4 && Math.Abs(d[ip]) + g == Math.Abs(d[ip])
                        && Math.Abs(d[iq]) + g == Math.Abs(d[iq]))
                            a[ip, iq] = 0.0;
                        else if (Math.Abs(a[ip, iq]) > tresh)
                        {
                            h = d[iq] - d[ip];
                            if (Math.Abs(h) + g == Math.Abs(h))
                                t = (a[ip, iq]) / h;
                            else
                            {
                                theta = 0.5 * h / (a[ip, iq]);
                                t = 1.0 / (Math.Abs(theta) + Math.Sqrt(1.0 + theta * theta));
                                if (theta < 0.0) t = -t;
                            }
                            c = 1.0 / Math.Sqrt(1 + t * t);
                            s = t * c;
                            tau = s / (1.0 + c);
                            h = t * a[ip, iq];
                            z[ip] -= h;
                            z[iq] += h;
                            d[ip] -= h;
                            d[iq] += h;
                            a[ip, iq] = 0.0;
                            for (j = 1; j <= ip - 1; j++)
                            {
                                ROTATE(a, j, ip, j, iq, tau, s);
                            }
                            for (j = ip + 1; j <= iq - 1; j++)
                            {
                                ROTATE(a, ip, j, j, iq, tau, s);
                            }
                            for (j = iq + 1; j <= n; j++)
                            {
                                ROTATE(a, ip, j, iq, j, tau, s);
                            }
                            for (j = 1; j <= n; j++)
                            {
                                ROTATE(v, j, ip, j, iq, tau, s);
                            }
                            ++nrot;
                        }
                    }
                }
                for (ip = 1; ip <= n; ip++)
                {
                    b[ip] += z[ip];
                    d[ip] = b[ip];
                    z[ip] = 0.0;
                }
            }
            //printf("Too many iterations in routine JACOBI");
        }


        private static void ROTATE(double[,] a, int i, int j, int k, int l, double tau, double s)
        {
            double g, h;
            g = a[i, j]; h = a[k, l]; a[i, j] = g - s * (h + g * tau);
            a[k, l] = h + s * (g - h * tau);
        }
        #endregion

        /// <summary>
        /// 中心点
        /// </summary>
        /// <param name="points"></param>
        /// <returns></returns>
        public static Vector2F Center(Vector2F[] points)
        {
            Vector2F center = Vector2F.Zero;
            float sumx = 0, sumy = 0;
            foreach (var item in points)
            {
                sumx += item.X;
                sumy += item.Y;
            }
            center.X = sumx / points.Length;
            center.Y = sumy / points.Length;
            return center;
        }

    }
}
